from rtde_control import RTDEControlInterface as RTDEControl

print("code running")
rtde_c = RTDEControl("192.168.111.130")
print("variable rtde_c is signed")

velocity = 0.5
acceleration = 0.5
blend_1 = 0.0
blend_2 = 0.02
blend_3 = 0.0
# path_pose1 = [-0.143, -0.435, 0.20, -0.001, 3.12, 0.04, velocity, acceleration, blend_1]
# path_pose2 = [-0.143, -0.51, 0.21, -0.001, 3.12, 0.04, velocity, acceleration, blend_2]
# path_pose3 = [-0.32, -0.61, 0.31, -0.001, 3.12, 0.04, velocity, acceleration, blend_2]
# path = [path_pose1, path_pose2, path_pose3]

path = [
    [-0.143, -0.435, 0.20, 0, 3.1415926, 0, velocity, acceleration, blend_1],
    [-0.143, -0.51, 0.21, 0, 3.1415926, 0, velocity, acceleration, blend_2],
    [-0.32, -0.61, 0.31, 0, 3.1415926, 0, velocity, acceleration, blend_2],
    [0.252406, -0.36899, 0.165344, 0, 3.1415926, 0, velocity, acceleration, blend_3],
]

# Send a linear path with blending in between - (currently uses separate script)
print(
    rtde_c.moveL(path)
)
# print(
#     rtde_c.moveL([
#         0.252406, -0.36899, 0.165344, 0, 3.14, 0, velocity, acceleration, blend_3
#     ])
# )
rtde_c.stopScript()
